#include<Math/polynomial.h>
#include <Matrix/LB_Matrix.h>
#define Polynomial LB_Polynomial
#define factorial LB_factorial



// 创建一个多项式
void resetpolynomial(int degree,Polynomial* p) {
    p->degree = degree;
    SAFE_FREE(p->coefficients);
    p->coefficients = (double *)malloc((degree + 1) * sizeof(double));
    memset(p->coefficients,0,sizeof(double)*(degree+1)); 
    // return p;
}

// 设置多项式的系数
void setcoefficient(Polynomial *p, int index, double value) {
    if (index > p->degree) {
        printf("Index out of range\n");
        return;
    }
    p->coefficients[index] = value;
}

// 打印多项式
void printpolynomial(Polynomial p) {
    for (int i = p.degree; i >= 0; i--) {
        if (p.coefficients[i] != 0) {
            printf("%gx^%d ", p.coefficients[i], i);
        }
    }
    printf("\n");
}

// 多项式相加
void addpolynomials(Polynomial* p1, Polynomial* p2,Polynomial* result) 
{
    int maxDegree = (p1->degree > p2->degree) ? p1->degree : p2->degree;
    // resetpolynomial(maxDegree,result);
    result->coefficients=(double*)realloc(result->coefficients,
        sizeof(double)*maxDegree);

    for (int i = 0; i <= maxDegree; i++) {
        double coefficient = 0;
        if (i <= p1->degree) {
            coefficient += p1->coefficients[i];
        }
        if (i <= p2->degree) {
            coefficient += p2->coefficients[i];
        }
        result->coefficients[i] = coefficient;
    }

    result->degree=maxDegree;
    //return result;
}

// 多项式相乘
void multiplypolynomials(Polynomial* p1, Polynomial* p2,Polynomial*result) {
    int resultDegree = p1->degree + p2->degree;
    resetpolynomial(resultDegree,result);

    for (int i = 0; i <= p1->degree; i++) 
    {
        for (int j = 0; j <= p2->degree; j++)
        {
            result->coefficients[i + j] += p1->coefficients[i] * p2->coefficients[j];
        }
    }
    // return result;
}


static inline int solve1(double a1,double a0,double roots[][2]){
    if(double_equal_zero(a1)){return 0;}
    roots[0][0]=-a0/a1;
    roots[0][1]=0;
    return 1;
}

static int solve2(double a2,double a1,double a0,double roots[][2])
{
    double d;
    if(double_equal_zero(a2)){return solve1(a1,a0,roots);}

    d=a1*a1-4*a0*a2;
    a1/=(2*a2);
    if(d<0){
        d=sqrt(-d)/(2*a2);
        roots[0][0]=roots[1][0]=-a1;
        roots[0][1]=-d;
        roots[1][1]= d;
    }
    else{
        d=sqrt(d)/(2*a2);
        roots[0][1]=roots[1][1]=0;
        roots[0][0]=-a1-d;
        roots[1][0]=-a1+d;
    }
    return 2;
}
// // Solution taken from: http://mathworld.wolfram.com/CubicFormula.html
// // and http://www.csit.fsu.edu/~burkardt/f_src/subpak/subpak.f90
#define SQRT_3 1.7320508075688772935

static int solve3(double a3,double a2,double a1,double a0,double roots[][2])
{
    double q,r,r2,q3;
    if(double_equal_zero(a3)){return solve2(a2,a1,a0,roots);}
    a2/=a3;
    a1/=a3;
    a0/=a3;
    q=-(3*a1-a2*a2)/9.0;
    r=-(9*a2*a1-27*a0-2*a2*a2*a2)/54.0;
    r2=r*r;
    q3=q*q*q;

    if(r2<q3){
        double sqrQ=sqrt(q);
        double theta = acos ( r / (sqrQ*q) );
        double cTheta=cos(theta/3)*sqrQ;
        double sTheta=sin(theta/3)*sqrQ*SQRT_3/2;
        roots[0][1]=roots[1][1]=roots[2][1]=0;
        roots[0][0]=-2*cTheta;
        roots[1][0]=-2*(-cTheta*0.5-sTheta);
        roots[2][0]=-2*(-cTheta*0.5+sTheta);
    }
    else{
        double s1,s2,sqr=sqrt(r2-q3);
        double t;
        t=-r+sqr;
        if(t<0){s1=-pow(-t,1.0/3);}
        else{s1=pow(t,1.0/3);}
        t=-r-sqr;
        if(t<0){s2=-pow(-t,1.0/3);}
        else{s2=pow(t,1.0/3);}
        roots[0][1]=0;
        roots[0][0]=s1+s2;
        s1/=2;
        s2/=2;
        roots[1][0]= roots[2][0]=-s1-s2;
        roots[1][1]= SQRT_3*(s1-s2);
        roots[2][1]=-roots[1][1];
    }
    roots[0][0]-=a2/3;
    roots[1][0]-=a2/3;
    roots[2][0]-=a2/3;
    return 3;
}
#undef SQRT_3
//下面的solve4代码有问题
// 
static int solve4(double a4, double a3, double a2, double a1, double a0, double roots[][2]) {
    if (double_equal_zero(a4)) {
        return solve3(a3, a2, a1, a0, roots);
    }

    // 使用费拉里公式将四次方程转换为三次方程
    double p = (8 * a4 * a3 - 3 * a2 * a2) / (8 * a4 * a4);
    double q = (a3 * a3 * a3 - 4 * a4 * a3 * a2 + 8 * a4 * a4 * a1) / (8 * a4 * a4 * a4);
    double r = (3 * a2 * a2 * a2 - 12 * a4 * a2 * a1 + 16 * a4 * a4 * a0) / (8 * a4 * a4 * a4);

    // 使用 Cardano 公式求解三次方程
    double u, v, w;
    double delta = q * q / 4 + r * r * r / 27;
    if (fabs(delta) <= 1e-9) 
    {
        u = 2 * cbrt(-q / 2);
        v = -0.5 * cbrt(-q / 2);
        w = -0.5 * cbrt(-q / 2);
    } 
    else if (delta > 0) {
        double s = cbrt(-q / 2 + sqrt(delta));
        double t = cbrt(-q / 2 - sqrt(delta));
        u = s + t;
        v = -0.5 * (s + t);
        w = 0.5 * sqrt(3) * (s - t);
    }
    else 
    {
        double theta = acos(-q / (2 * sqrt(-r * r * r / 27)));
        u = 2 * sqrt(-r / 3) * cos(theta / 3);
        v = 2 * sqrt(-r / 3) * cos((theta + 2 * M_PI) / 3);
        w = 2 * sqrt(-r / 3) * cos((theta + 4 * M_PI) / 3);
    }

    // 计算四次方程的根
    roots[0][0] = (u + v) / 2;
    roots[0][1] = (u - v) / 2;
    roots[1][0] = (u + w) / 2;
    roots[1][1] = (u - w) / 2;
    roots[2][0] = (v + w) / 2;
    roots[2][1] = (v - w) / 2;
    roots[3][0] = -0.5 * (u + v + w);
    roots[3][1] = 0.5 * sqrt(3) * (u - v - w);

    return 4;
}

// double Angle(const double in[2]){
//     if((in[0]*in[0]+in[1]*in[1])==0.0){return 0;}
//     else{return atan2(in[1],in[0]);}
// }

// void Sqrt(const double in[2],double out[2]){
//     double r=sqrt(sqrt(in[0]*in[0]+in[1]*in[1]));
//     double a=Angle(in)*0.5;
//     out[0]=r*cos(a);
//     out[1]=r*sin(a);
// }
// void Add(const double in1[2],const double in2[2],double out[2]){
//     out[0]=in1[0]+in2[0];
//     out[1]=in1[1]+in2[1];
// }
// void Subtract(const double in1[2],const double in2[2],double out[2]){
//     out[0]=in1[0]-in2[0];
//     out[1]=in1[1]-in2[1];
// }
// void Multiply(const double in1[2],const double in2[2],double out[2]){
//     out[0]=in1[0]*in2[0]-in1[1]*in2[1];
//     out[1]=in1[0]*in2[1]+in1[1]*in2[0];
// }
// void Divide(const double in1[2],const double in2[2],double out[2]){
//     double temp[2];
//     double l=in2[0]*in2[0]+in2[1]*in2[1];
//     temp[0]= in2[0]/l;
//     temp[1]=-in2[1]/l;
//     Multiply(in1,temp,out);
// }

// int solve4(double a4,double a3,double a2,double a1,double a0,double roots[][2])
// {
//     double R[2],D[2],E[2],R2[2];
//     if(fabs(a4)<1e-9){return solve3(a3,a2,a1,a0,roots);}
//     a3/=a4;
//     a2/=a4;
//     a1/=a4;
//     a0/=a4;
//     solve3(1.0,-a2,a3*a1-4.0*a0,-a3*a3*a0+4.0*a2*a0-a1*a1,roots);
//     R2[0]=a3*a3/4.0-a2+roots[0][0];
//     R2[1]=0;
//     Sqrt(R2,R);
//     if(fabs(R[0])>10e-8){
//         double temp1[2],temp2[2];
//         double p1[2],p2[2];
//         p1[0]=a3*a3*0.75-2.0*a2-R2[0];
//         p1[1]=0;
//         temp2[0]=((4.0*a3*a2-8.0*a1-a3*a3*a3)/4.0);
//         temp2[1]=0;
//         Divide(temp2,R,p2);
//         Add     (p1,p2,temp1);
//         Subtract(p1,p2,temp2);
//         Sqrt(temp1,D);
//         Sqrt(temp2,E);
//     }
//     else{
//         R[0]=R[1]=0;
//         double temp1[2],temp2[2];
//         temp1[0]=roots[0][0]*roots[0][0]-4.0*a0;
//         temp1[1]=0;
//         Sqrt(temp1,temp2);
//         temp1[0]=a3*a3*0.75-2.0*a2+2.0*temp2[0];
//         temp1[1]=                  2.0*temp2[1];
//         Sqrt(temp1,D);
//         temp1[0]=a3*a3*0.75-2.0*a2-2.0*temp2[0];
//         temp1[1]=                 -2.0*temp2[1];
//         Sqrt(temp1,E);
//     }
//     roots[0][0]=-a3/4.0+R[0]/2.0+D[0]/2.0;
//     roots[0][1]=        R[1]/2.0+D[1]/2.0;
//     roots[1][0]=-a3/4.0+R[0]/2.0-D[0]/2.0;
//     roots[1][1]=        R[1]/2.0-D[1]/2.0;
//     roots[2][0]=-a3/4.0-R[0]/2.0+E[0]/2.0;
//     roots[2][1]=       -R[1]/2.0+E[1]/2.0;
//     roots[3][0]=-a3/4.0-R[0]/2.0-E[0]/2.0;
//     roots[3][1]=       -R[1]/2.0-E[1]/2.0;
//     return 4;
// }
void simplifypolynomial(Polynomial* result)
{
    int d=result->degree;
    for(int i=result->degree;i>=0;i--)
    {
        if(fabs(result->coefficients[i])<1e-9)
        {
            d--;
            continue;
        }
        break;
    }
    double* newCoefficients = NULL;
    if (d >= 0) {
        newCoefficients = (double*)malloc((d + 1) * sizeof(double));
        if (newCoefficients) {
            memset(newCoefficients, 0, sizeof(double) * (d + 1));
            for (int i = 0; i <= d; i++) {
                newCoefficients[i] = result->coefficients[i];
            }
        }
    }

    free(result->coefficients);

    result->degree=d;
    result->coefficients=newCoefficients;
}
// // 求解多项式等于0的复数解

int getsolutions(Polynomial *p,const double c,
    double r[][2])
{
    // double r[4][2];
    int rCount=0;
    double* coefficients=p->coefficients;
    switch(p->degree){
    case 1:
        rCount=solve1(coefficients[1],coefficients[0]-c,r);
        break;
    case 2:
        rCount=solve2(coefficients[2],coefficients[1],coefficients[0]-c,r);
        break;
    case 3:
        rCount=solve3(coefficients[3],coefficients[2],coefficients[1],coefficients[0]-c,r);
        break;
    // case 4:
    //     rCount=Factor(coefficients[4],coefficients[3],coefficients[2],coefficients[1],coefficients[0]-c,r,EPS);
    //     break;
    default:
        printf("Can't solve polynomial of degree: %d\n",p->degree);
    }
    return rCount;
}


// 多项式微分
void differentiatepolynomial(Polynomial* p,Polynomial*result) 
{
    if(p->degree<0){return;}
    double *coefficients = p->coefficients;

    if(result==NULL)
    {
        if(p->degree==0)
        {
            p->coefficients[0]=0;
            return ;
        }
        int degree=p->degree;
        Polynomial tp;
        lb_polynomial_initn(&tp,degree-1);

        for (int i = 1; i <=degree; i++) 
        {
            tp.coefficients[i - 1] = coefficients[i] * i;
        }
        resetpolynomial(degree-1,p);
        memmove(p->coefficients,tp.coefficients,sizeof(double)*(degree));
        
        free(tp.coefficients);
        return;
    }
    if(p->degree==0){
        resetpolynomial(0,result);
        return ;
    }
    resetpolynomial(p->degree-1,result);
    for (int i = 1; i <= p->degree; i++) 
    {
        result->coefficients[i - 1] = coefficients[i] * i;
    }
}

// 多项式积分
void  integratepolynomial(Polynomial* p,Polynomial*result) 
{
    int degree = p->degree;

    if(result==NULL)
    {
        //请补充
        Polynomial tp;
        lb_polynomial_initn(&tp,degree+1);

        for (int i = 0; i <= degree; i++) {
            tp.coefficients[i + 1] = p->coefficients[i] / (double)(i + 1);
        }
        resetpolynomial(degree+1,p);
        memmove(p->coefficients,tp.coefficients,sizeof(double)*(degree));
        free(tp.coefficients);
        return ;
    }
    resetpolynomial(degree + 1,result);
    for (int i = 0; i <= degree; i++) {
        result->coefficients[i + 1] = p->coefficients[i] / (double)(i + 1);
    }
}



// 
// 从n个选取m个，总共有多少种方法
//

static inline int get_Cnm(int n,int m)
{
    int re=1;
    for(int i=0;i<m;i++)
    {
        re=(n-i);     
    }
    re=re/factorial(m);
    return re;
}

static inline void  polynomial_plus_pow_xan(Polynomial* p,double a,double fac,int degree)
{
    // 从degree中选
    // 0 .. degree 
    double temp=1;
    int cnm=1;
    for(int i=degree;i>=0;i--)
    {
        p->coefficients[i]+=(fac*temp*cnm);
        temp*=a;
        int j=degree-i+1;
        cnm*=((degree-j+1) /j);
    }
} 
// 
// x根正方向移动是 x-offset
// 负方向移动是 x+offset
//
void polynomialshift_root(Polynomial* p,double offset,Polynomial* result)
{
    if(result==NULL)
    {
        Polynomial tp;
        lb_polynomial_initn(&tp,p->degree);

        for (int i = 0; i <= p->degree; i++) 
        {
            polynomial_plus_pow_xan(&tp,-offset,p->coefficients[i],i);
            // p->coefficients[i]*=fac;
        }
        memmove(p->coefficients,tp.coefficients,sizeof(double)*(p->degree+1));
        free(tp.coefficients);
        return ;
    }

    memset(result->coefficients,0,sizeof(double)*(p->degree+1));

    // memmove(result->coefficients,p->coefficients,sizeof(double)*(p->degree+1));
    for (int i = 0; i <= p->degree; i++)
    {
        polynomial_plus_pow_xan(result,-offset,p->coefficients[i],i);
    }

}


double get_polynomial_fun_value(Polynomial* p,double x)
{
    double temp=x;
    double re=p->coefficients[0];
    for (int i = 1; i <= p->degree; i++)
    {
        re+=temp*p->coefficients[i];
        temp*=x;
    }
    return re;
}
